/*
 * This file is part of Cleanflight and Betaflight.
 *
 * Cleanflight and Betaflight are free software. You can redistribute
 * this software and/or modify this software under the terms of the
 * GNU General Public License as published by the Free Software
 * Foundation, either version 3 of the License, or (at your option)
 * any later version.
 *
 * Cleanflight and Betaflight are distributed in the hope that they
 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this software.
 *
 * If not, see <http://www.gnu.org/licenses/>.
 */

#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include <limits.h>

#include "platform.h"
#include "arm_math.h"
#include "build/debug.h"

#include "common/maths.h"
#include "common/filter.h"

#include "fc/runtime_config.h"

#include "flight/position.h"
#include "flight/imu.h"
#include "flight/pid.h"
#include "flight/alt_hold_multirotor.h"

#include "io/gps.h"

#include "scheduler/scheduler.h"

#include "sensors/sensors.h"
#include "sensors/barometer.h"
#include "sensors/rangefinder.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"

static float displayAltitudeCm = 0.0f;
static bool altitudeAvailable = false;

static float zeroedAltitudeCm = 0.0f;
static float zeroedAltitudeDerivative = 0.0f;

static pt2Filter_t altitudeLpf;
static pt2Filter_t altitudeDerivativeLpf;

#ifdef USE_VARIO
static int16_t estimatedVario = 0; // in cm/s
#endif

void positionInit(void)
{
    const float sampleTimeS = HZ_TO_INTERVAL(TASK_ALTITUDE_RATE_HZ);

    const float altitudeCutoffHz = positionConfig()->altitude_lpf / 100.0f;
    const float altitudeGain = pt2FilterGain(altitudeCutoffHz, sampleTimeS);
    pt2FilterInit(&altitudeLpf, altitudeGain);

    const float altitudeDerivativeCutoffHz = positionConfig()->altitude_d_lpf / 100.0f;
    const float altitudeDerivativeGain = pt2FilterGain(altitudeDerivativeCutoffHz, sampleTimeS);
    pt2FilterInit(&altitudeDerivativeLpf, altitudeDerivativeGain);
}

typedef enum {
    DEFAULT = 0,
    BARO_ONLY,
    GPS_ONLY
} altitudeSource_e;

PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 6);

PG_RESET_TEMPLATE(positionConfig_t, positionConfig,
    .altitude_source = DEFAULT,
    .altitude_prefer_baro = 100, // percentage 'trust' of baro data
    .altitude_lpf = 300,
    .altitude_d_lpf = 100,
);

//-------------------------------TODO数据融合---------------------------

float positionKalmanZ;
float velocityKalmanZ;
/*
	dt：传递给A和B的时间  
    acc: kalman滤波的输入1
    atti: kalman滤波的输入2
	Q：	根据经验给定过程噪声协方差矩阵，这里是一个2X2矩阵
	R：	根据经验给定测量噪声协方差矩阵 
	pos_esti: kalman滤波结果输出1
	vel_esti: kalman滤波结果输出2
*/
void KalmanPosZEsti(float dt, 	// 时间间隔
					float accz,		// 地球系下，Z轴加速度 (m/s^2)
					float atti,		// 地球系下，气压计（或测距仪）测量的高度 (m)
					const float R,
					const float Q,
					float *pos_esti, // 位置估计
					float *vel_esti)
{
	 /*定义保存矩阵A,B值的一维数组*/
	 static float A[4] = { 0 }; //一维数组
	 static float B[2] = { 0 };
	 static float H[2] = { 0 };
	 static float HPHT[1] = {0} ; // 特例
	 static float Q_[4] = {0};
	 
	 static float X_[2] = {0,0}; // 状态变量的先验估计初始值为{0,0}
	 static float X[2] = {0.0f,0.0f}; // 状态变量的后验估计初始值为{0,0}
	 static float P_[4] = {0,0,0,0}; // 误差协方差矩阵先验的初始值为{0,0,0,0}
	 static float P[4] = {1.0,0,0,1.0}; // 误差协方差矩阵后验的初始值为{1,0,0,1}
	 static float AX[2] = {0,0};
	 static float AHATP[4] = {0};
	 static float AHATPAT[4] = {0};
	 static float PHT[2] = { 0 };
	 static float KH[4] = {0};
	 
	 static float32_t K[2] = {0};
	 static float HK[2] = { 0 };
	 static float I_KH[2] = { 0 };
	 static float I[4] = {1,0,0,1};
	 
	 static float u = 0; // 定义控制变量，也就是系统的输入
	 
	 // 定义矩阵实例
	 arm_matrix_instance_f32 _A; // 定义一个矩阵实例 _A，保存一维数组A的值
	 arm_matrix_instance_f32 _A_T; // 定义一个矩阵实例 _A_T ，保存一维数组A的值
	 arm_matrix_instance_f32 _B; // 定义一个矩阵实例 _B，保存一维数组B的值
	 arm_matrix_instance_f32 _H; // 定义一个矩阵实例 _H，保存一维数组H的值
	 arm_matrix_instance_f32 _HT; // 定义一个矩阵实例 _HT，保存一维数组H的值
	 arm_matrix_instance_f32 hatx_; // Xk的先验估计
	 arm_matrix_instance_f32 hatx; // Xk的后验估计
	 arm_matrix_instance_f32 hatP_; // P的先验估计 hatP_
	 arm_matrix_instance_f32 hatP; // P的后验估计 hatP
	 arm_matrix_instance_f32 Kk; // kalman增益
	 arm_matrix_instance_f32 _KH;
	 arm_matrix_instance_f32 _I_KH;
	 arm_matrix_instance_f32 _I;
	 
	 // 临时的中间矩阵
	 arm_matrix_instance_f32 A_hatX; // Ak * hatXk-1
	 arm_matrix_instance_f32 A_hatP; // Ak * hatPk-1
	 arm_matrix_instance_f32 A_hatPAT; // Ak * hatPk-1 * AT
	 arm_matrix_instance_f32 HkhatPk_; 
	 arm_matrix_instance_f32 hatPk_HkT; // hatPk_ * HkT
	 arm_matrix_instance_f32 HP_HT; //H*P_*H'
	 
	 arm_matrix_instance_f32 _Q;
	 Q_[0] = Q;
	 Q_[1] = 0;
	 Q_[2] = 0;
	 Q_[3] = Q;
	 arm_mat_init_f32(&_Q, 2, 2, Q_); // _A 矩阵是2X2的矩阵，里面的值是A
	 	 
	 u = accz; // 将加速度
	 
	 A[0] = 1; A[1] = dt;
	 A[2] = 0; A[3] = 1;
	 
	 B[0] = 0.5*dt*dt * u;
	 B[1] = dt * u;
	 
	 H[0] = 1;
	 H[1] = 0;
	 
	 // 初始化矩阵实例
	 arm_mat_init_f32(&_A, 2, 2, A); // _A 矩阵是2X2的矩阵，里面的值是A
	 arm_mat_init_f32(&_A_T, 2, 2, A); // _A_T 矩阵是2X2的矩阵，里面的值是A
	 arm_mat_init_f32(&_B, 2, 1, B); // _B 矩阵是2X1的矩阵，里面的值是B
	 arm_mat_init_f32(&_H, 1, 2, H); // _H 矩阵是1X2的矩阵，里面的值是H
	 arm_mat_init_f32(&_HT, 2, 1, H); // _HT 矩阵是2X1的矩阵，里面的值是H
	 arm_mat_init_f32(&HP_HT, 1, 1, HPHT); //
	 arm_mat_init_f32(&hatx_, 2, 1, X_); // hatx_ 矩阵是2X1的矩阵，里面的值是X_
     arm_mat_init_f32(&hatx, 2, 1, X); // hatx 矩阵是2X1的矩阵，里面的值是X
	 arm_mat_init_f32(&A_hatX, 2, 1, AX); // A_hatX 矩阵是2X1的矩阵，里面的值是AX
	 arm_mat_init_f32(&hatP_, 2, 2, P_); // hatP_ 矩阵是2X2的矩阵，里面的值是P_
	 arm_mat_init_f32(&hatP, 2, 2, P); // hatP 矩阵是2X2的矩阵，里面的值是P
	 arm_mat_init_f32(&A_hatP, 2, 2, AHATP);  
	 arm_mat_init_f32(&A_hatPAT, 2, 2, AHATPAT);  
	 arm_mat_init_f32(&Kk, 2, 1, K);
	 arm_mat_init_f32(&hatPk_HkT, 2, 1, PHT);
	 arm_mat_init_f32(&HkhatPk_, 1, 2, HK);
	 arm_mat_init_f32(&_KH, 2, 2, KH);
	 arm_mat_init_f32(&_I_KH, 2, 2, I_KH);
	 arm_mat_init_f32(&_I, 2, 2, I);
	 
	 //kalman公式1: xk的先验估计 
	 arm_mat_mult_f32(&_A, &hatx, &A_hatX); //  A_hatX = _A * hatx
	 arm_mat_add_f32(&A_hatX, &_B, &hatx_); // 卡尔曼滤波公式1，hatx_是先验估计
	 
	 //kalman公式2：误差协方差矩阵的先验估计
	 arm_mat_trans_f32(&_A, &_A_T);					// A' (2x2)
	 arm_mat_mult_f32(&_A, &hatP, &A_hatP); //  A_hatP = _A * hatP
	 arm_mat_mult_f32(&A_hatP, &_A_T, &A_hatPAT); //  A_hatPAT = A_hatP * _A_T
	 arm_mat_add_f32(&A_hatPAT, &_Q, &hatP_); // hatP_ = A_hatPAT + Q
	 
	 // kalman公式3：Kk = hatPk_ * HkT*(Hk*hatPk_HT + R)’
	 arm_mat_mult_f32(&_H, &hatP_, &HkhatPk_);
	 arm_mat_trans_f32(&_H, &_HT);					
	 arm_mat_mult_f32(&HkhatPk_, &_HT, &HP_HT);		
	 HPHT[0] += R;															
	 HPHT[0] = 1 / HPHT[0];											
	 arm_mat_mult_f32(&hatP_, &_HT, &hatPk_HkT);		
	 arm_mat_mult_f32(&hatPk_HkT, &HP_HT, &Kk);	
	 
	 
	 // 卡尔曼公式4
	 arm_mat_mult_f32(&_H, &hatx_, &HP_HT);	
	 HPHT[0] = atti - HPHT[0];							
	 arm_mat_mult_f32(&Kk, &HP_HT, &A_hatX);	
	 arm_mat_add_f32(&hatx_, &A_hatX, &hatx);	

	 // 卡尔曼公式5
	 arm_mat_mult_f32(&Kk, &_H, &_KH);		
	 arm_mat_sub_f32(&_I, &_KH, &_I_KH);			
	 arm_mat_mult_f32(&_I_KH, &hatP_, &hatP);		
	 
	 *pos_esti =  hatx.pData[0];// 位置估计
	 *vel_esti =  hatx.pData[1];
}

#if defined(USE_BARO) || defined(USE_GPS)
void calculateEstimatedAltitude(void)
{
    static bool wasArmed = false;
    static bool useZeroedGpsAltitude = false; // whether a zero for the GPS altitude value exists
    static float gpsAltCm = 0.0f; // will hold last value on transient loss of 3D fix
    static float gpsAltOffsetCm = 0.0f;
    static float baroAltOffsetCm = 0.0f;
    static float newBaroAltOffsetCm = 0.0f;
    static bool useZeroedSonarAltitude = false;

    float baroAltCm = 0.0f;
    float gpsTrust = 0.3f; // if no pDOP value, use 0.3, intended range 0-1;
    bool haveBaroAlt = false; // true if baro exists and has been calibrated on power up
    bool haveGpsAlt = false; // true if GPS is connected and while it has a 3D fix, set each run to false
    int32_t sonarAltCm = 0;
    bool haveSonarAlt = false;
 
    // *** Get sensor data

#ifdef USE_RANGEFINDER
    #if defined(USE_RANGEFINDER_HCSR04)
        //超声波...
        if (sensors(SENSOR_RANGEFINDER) && rangefinderIsHealthy()){
            sonarAltCm = rangefinderGetLatestAltitude();
            if (sonarAltCm > 0){
                if(useZeroedSonarAltitude){
                    // 从气压计 => 超声波，需要重新设置target
                    altHoldResetByAltitude(sonarAltCm);
                    useZeroedSonarAltitude = false;
                }
                haveSonarAlt = true;
            }else{
                //小于0为异常，超声波维持原有高度
            }
        }
    #endif 
#endif

#ifdef USE_BARO
    if (sensors(SENSOR_BARO)) {
        baroAltCm = getBaroAltitude();
        haveBaroAlt = true; // false only if there is no sensor on the board, or it has failed
        if(haveSonarAlt){
            //如果超声波数据有效，则使用超声波数据定高
            baroAltCm = sonarAltCm;
        }else{
            if(!useZeroedSonarAltitude){
                // 从超声波 => 气压计，需要重新设置target
                altHoldResetByAltitude(baroAltCm);
                useZeroedSonarAltitude = true;
            }
        }

        #ifdef USE_KALMAN_FILTER
            vector3_t accNed;
            calculateRawAccBodyToNed(&accNed);
            KalmanPosZEsti(0.01f,accNed.z,baroAltCm,1,4e-8f,&positionKalmanZ,&velocityKalmanZ);
            baroAltCm = positionKalmanZ;
        #endif
  
    }
#endif


#ifdef USE_GPS
    if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
        // GPS_FIX means a 3D fix, which requires min 4 sats.
        // On loss of 3D fix, gpsAltCm remains at the last value, haveGpsAlt becomes false, and gpsTrust goes to zero.
        gpsAltCm = gpsSol.llh.altCm; // static, so hold last altitude value if 3D fix is lost to prevent fly to moon
        haveGpsAlt = true; // goes false and stays false if no 3D fix
        if (gpsSol.dop.pdop != 0) {
            // pDOP of 1.0 is good.  100 is very bad.  Our gpsSol.dop.pdop values are *100
            // When pDOP is a value less than 3.3, GPS trust will be stronger than default.
            gpsTrust = 100.0f / gpsSol.dop.pdop;
            // *** TO DO - investigate if we should use vDOP or vACC with UBlox units;
        }
        // always use at least 10% of other sources besides gps if available
        gpsTrust = MIN(gpsTrust, 0.9f);
    }
#endif

    //  ***  DISARMED  ***
    if (!ARMING_FLAG(ARMED)) {
        if (wasArmed) { // things to run once, on disarming, after being armed
            useZeroedGpsAltitude = false; // reset, and wait for valid GPS data to zero the GPS signal
            wasArmed = false;
        }

        newBaroAltOffsetCm = 0.2f * baroAltCm + 0.8f * newBaroAltOffsetCm; // smooth some recent baro samples
        displayAltitudeCm = baroAltCm - baroAltOffsetCm; // if no GPS altitude, show un-smoothed Baro altitude in OSD and sensors tab, using most recent offset.

        if (haveGpsAlt) { // watch for valid GPS altitude data to get a zero value from
            gpsAltOffsetCm = gpsAltCm; // update the zero offset value with the most recent valid gps altitude reading
            useZeroedGpsAltitude = true; // we can use this offset to zero the GPS altitude on arming
            if (!(positionConfig()->altitude_source == BARO_ONLY)) {
                displayAltitudeCm = gpsAltCm; // estimatedAltitude shows most recent ASL GPS altitude in OSD and sensors, while disarmed
            }
        }
        zeroedAltitudeCm = 0.0f; // always hold relativeAltitude at zero while disarmed
        DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAltCm / 100.0f); // Absolute altitude ASL in metres, max 32,767m
    //  ***  ARMED  ***
    } else {
        if (!wasArmed) { // things to run once, on arming, after being disarmed
            baroAltOffsetCm = newBaroAltOffsetCm;
            wasArmed = true;
        }

        baroAltCm -= baroAltOffsetCm; // use smoothed baro with most recent zero from disarm period

        if (haveGpsAlt) { // update relativeAltitude with every new gpsAlt value, or hold the previous value until 3D lock recovers
            if (!useZeroedGpsAltitude && haveBaroAlt) { // armed without zero offset, can use baro values to zero later
                gpsAltOffsetCm = gpsAltCm - baroAltCm; // not very accurate
                useZeroedGpsAltitude = true;
            }
            if (useZeroedGpsAltitude) { // normal situation
                zeroedAltitudeCm = gpsAltCm - gpsAltOffsetCm; // now that we have a GPS offset value, we can use it to zero relativeAltitude
            }
        } else {
            gpsTrust = 0.0f;
            // TO DO - smoothly reduce GPS trust, rather than immediately dropping to zero for what could be only a very brief loss of 3D fix
        }
        DEBUG_SET(DEBUG_ALTITUDE, 2, lrintf(zeroedAltitudeCm / 10.0f)); // Relative altitude above takeoff, to 0.1m, rolls over at 3,276.7m

        // Empirical mixing of GPS and Baro altitudes
        if (useZeroedGpsAltitude && (positionConfig()->altitude_source == DEFAULT || positionConfig()->altitude_source == GPS_ONLY)) {
            if (haveBaroAlt && positionConfig()->altitude_source == DEFAULT) {
                // mix zeroed GPS with Baro altitude data, if Baro data exists if are in default altitude control mode
                const float absDifferenceM = fabsf(zeroedAltitudeCm - baroAltCm) / 100.0f * positionConfig()->altitude_prefer_baro / 100.0f;
                if (absDifferenceM > 1.0f) { // when there is a large difference, favour Baro
                    gpsTrust /=  absDifferenceM;
                }
                zeroedAltitudeCm = zeroedAltitudeCm * gpsTrust + baroAltCm * (1.0f - gpsTrust);
            }
        } else if (haveBaroAlt && (positionConfig()->altitude_source == DEFAULT || positionConfig()->altitude_source == BARO_ONLY)) {
            zeroedAltitudeCm = baroAltCm; // use Baro if no GPS data, or we want Baro only
        }
    }

#ifdef USE_KALMAN_FILTER
    {
        zeroedAltitudeCm = positionKalmanZ;
        if (wasArmed) {
            displayAltitudeCm = positionKalmanZ; // while armed, show filtered relative altitude in OSD / sensors tab
        }
        zeroedAltitudeDerivative = velocityKalmanZ;
     
    }
#else
    zeroedAltitudeCm = pt2FilterApply(&altitudeLpf, zeroedAltitudeCm);
    // NOTE: this filter must receive 0 as its input, for the whole disarmed time, to ensure correct zeroed values on arming

    if (wasArmed) {
        displayAltitudeCm = zeroedAltitudeCm; // while armed, show filtered relative altitude in OSD / sensors tab
    }

    // *** calculate Vario signal
    static float previousZeroedAltitudeCm = 0.0f;
    zeroedAltitudeDerivative = (zeroedAltitudeCm - previousZeroedAltitudeCm) * TASK_ALTITUDE_RATE_HZ; // cm/s
    previousZeroedAltitudeCm = zeroedAltitudeCm;

    zeroedAltitudeDerivative = pt2FilterApply(&altitudeDerivativeLpf, zeroedAltitudeDerivative);
#endif

#ifdef USE_VARIO
    estimatedVario = lrintf(zeroedAltitudeDerivative);
    estimatedVario = applyDeadband(estimatedVario, 10); // ignore climb rates less than 0.1 m/s
#endif

    // *** set debugs
    DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust));
    DEBUG_SET(DEBUG_ALTITUDE, 1, lrintf(baroAltCm / 10.0f)); // Relative altitude above takeoff, to 0.1m, rolls over at 3,276.7m
#ifdef USE_VARIO
    DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedVario);
#endif
    DEBUG_SET(DEBUG_RTH, 1, lrintf(displayAltitudeCm / 10.0f));
    DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 2, lrintf(zeroedAltitudeCm));

    altitudeAvailable = haveGpsAlt || haveBaroAlt;
}

#endif //defined(USE_BARO) || defined(USE_GPS)

float getAltitudeCm(void)
{
    return zeroedAltitudeCm;
}

float getAltitudeDerivative(void)
{
    return zeroedAltitudeDerivative; // cm/s
}

bool isAltitudeAvailable(void) {
    return altitudeAvailable;
}

int32_t getEstimatedAltitudeCm(void)
{
    return lrintf(displayAltitudeCm);
}

#ifdef USE_GPS
float getAltitudeAsl(void)
{
    return gpsSol.llh.altCm;
}
#endif

#ifdef USE_VARIO
int16_t getEstimatedVario(void)
{
    return estimatedVario;
}
#endif
